package image;

import java.awt.Dimension;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Collections;

public class Manip {

	/**
	 * Find the (green) corners of a deformed rectangle in 4 quadrants of an image.
	 * @param deformedImage The image where the deformed rectangle exists. 
	 * The corners should be marked as green in the image.
	 * @param d The size of the image.
	 * @return The corners which were marked as green, in the 4 quadrants of the image, counter-clockwise. 
	 * The upper-right corner is first.
	 */
	public static V3F[] findCorners(byte[] deformedImage, Dimension d) {
		byte[] data = deformedImage;
		@SuppressWarnings("unchecked")
		ArrayList<Integer>[] cornerX = (ArrayList<Integer>[]) Array.newInstance(ArrayList.class, 4);
		@SuppressWarnings("unchecked")
		ArrayList<Integer>[] cornerY = (ArrayList<Integer>[]) Array.newInstance(ArrayList.class, 4);
		for(int i = 0 ; i<4; i++) {
			cornerX[i] = new ArrayList<Integer>();
			cornerY[i] = new ArrayList<Integer>();
		}

		for(int y = 0; y<d.height; y+=1) {
			for(int x = 0; x<d.width; x+=1) {
				int i = 3*(x+y*d.width);
				int r = data[i]&0xFF;
				int g = data[i+1]&0xFF;
				int b = data[i+2]&0xFF;
				
				final int quadrant;
				if(x < d.width/2)
					if(y < d.height/2)
						quadrant = 1;
					else
						quadrant = 2;
				else
					if(y < d.height/2)
						quadrant = 0;
					else
						quadrant = 3;
				if(g > r+16 && g > b+16) {
					cornerX[quadrant].add(x);
					cornerY[quadrant].add(y);
				}
			}
		}

		V3F[] mediansForCorners = new V3F[4];

		for(int i = 0 ; i<4; i++) {
			if (cornerX[i].size() == 0) {
				mediansForCorners[i] = new V3F((i==0||i==3)?d.width:0, (i==2||i==3)?d.height:0, 0);
				continue;
			}

			Collections.sort(cornerX[i]);
			Collections.sort(cornerY[i]);
			float medianX, medianY;
			if(cornerX[i].size()%2 == 0) { 
				medianX = (cornerX[i].get(cornerX[i].size()/2)+cornerX[i].get(cornerX[i].size()/2-1))/2.0f;
				medianY = (cornerY[i].get(cornerY[i].size()/2)+cornerY[i].get(cornerY[i].size()/2-1))/2.0f;
			}
			else {
				medianX = cornerX[i].get(cornerX[i].size()/2);
				medianY = cornerY[i].get(cornerY[i].size()/2);
			}
			mediansForCorners[i] = new V3F(medianX, medianY, 0);
		}
		return mediansForCorners;
	}
	/**
	 * Update the indicated grid from the image. The areas around the robot are not updated.
	 * @param grid The pathfinding grid that should be affected.
	 * @param contrastedImage The contrasted image. 0,0,0 is blocked, otherwise open. 
	 * The image data will be purplified around the robot. These areas are not updated in the grid.
	 * @param d The size of the image
	 * @param ax The position of the rear ball
	 * @param ay The position of the rear ball
	 * @param px The position of the robot center
	 * @param py The position of the robot center
	 * @param bx The position of the front ball
	 * @param by The position of the front ball
	 * @param robotSize The smoothed size of the robot, in image space
	 */
	public static void setOpenBlocked(Grid grid, byte[] contrastedImage, Dimension d, 
			double ax, double ay, double px, double py, double bx, double by, double robotSize) {
		double smoothedSize = robotSize;
		byte[] data = contrastedImage;
		for(int y = 0; y<d.height; y+=1) {
			for(int x = 0; x<d.width; x+=1) {
				
				int i = 3*(x+(d.height-1-y)*d.width);
				
				int rVal = data[i]&0xFF;
				int gVal = data[i+1]&0xFF;
				int bVal = data[i+2]&0xFF;
				
				if(		Calc.sqDist(x-ax, y-ay) < smoothedSize*smoothedSize*0.5 ||
						Calc.sqDist(x-px, y-py) < smoothedSize*smoothedSize*1.25 ||
						Calc.sqDist(x-bx, y-by) < smoothedSize*smoothedSize*1) {
					data[i  ] = (byte)(((data[i  ]&0xFF)+255)/2);
					data[i+1] = (byte)((data[i+1]&0xFF)/2);
					data[i+2] = (byte)(((data[i+2]&0xFF)+255)/2);
				}
				else if(rVal == 0 && gVal == 0 && bVal == 0) {
					grid.setSquare(grid.getGridX(x), grid.getGridY(y), Grid.BLOCKED);
				}
				else
					grid.setSquare(grid.getGridX(x), grid.getGridY(y), Grid.OPEN);
			}
		}
	}

}
